#ifndef __SPASOLVER__
#define __SPASOLVER__

#include <open_karto/Mapper.h>
#ifndef EIGEN_USE_NEW_STDVECTOR
#define EIGEN_USE_NEW_STDVECTOR
#endif // EIGEN_USE_NEW_STDVECTOR

#define EIGEN_DEFAULT_IO_FORMAT Eigen::IOFormat(10)

#include <Eigen/Eigen>
#include "sparse_bundle_adjustment/spa2d.h"

typedef std::vector<karto::Matrix3> CovarianceVector;

class SpaSolver : public karto::ScanSolver
{
public:
    SpaSolver();

    virtual ~SpaSolver();

    virtual void Clear();

    virtual void Compute();

    virtual const karto::ScanSolver::IdPoseVector &GetCorrections() const;

    virtual void AddNode(karto::Vertex <karto::LocalizedRangeScan> *pVertex);

    virtual void AddConstraint(karto::Edge <karto::LocalizedRangeScan> *pEdge);

private:
    karto::ScanSolver::IdPoseVector corrections;

    sba::SysSPA2d m_Spa;
};

#endif // __SPASOLVER__

